import numpy as np
import acado
import time
import matplotlib.pyplot as plt
from numpy import sin, cos,exp

import time
import rospy

pc_size=30

N=20 #timestep
NU=2
NOD=pc_size*3+3
NX=4
NY=6
NYN=4

NW=6
NWN=4

x0=np.zeros(NX).reshape(1,NX)
od=np.zeros(NOD)
y=np.zeros([N,NY])
u=np.zeros([N,NU])
Q=np.eye(NW)

Q[2,2]=10
Q[3,3]=10

W=np.transpose(np.tile(Q,N))

WN=Q[:-2,:-2]

X=np.tile(x0.reshape(NX,1),N+1).T



OD=np.ones([N+1,NOD])

OD[:,pc_size*0:pc_size*1]=200
OD[:,pc_size*1:pc_size*2]=0
OD[:,pc_size*2:pc_size*3]=-200

for i in range(pc_size):
    OD[:,pc_size*2:pc_size*3]=200-i*1
    

xlog=[]
for j in range(20):
    X,u=acado.mpc(0,1,x0,X,u,OD,y,W,WN,1)
    x0=X[1,:].reshape(1,NX)
    #OD[:,pc_size*1:pc_size*2]=200-j*20
    xlog.append(x0)
xlog_arr=np.vstack(xlog)

plt.plot(xlog_arr[:,0]/3.14*180)
plt.plot(xlog_arr[:,1]/3.14*180)
plt.legend(["yaw","pitch"])
plt.show()